package com.cadgit.bodymeter;

import android.app.Activity;
import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.view.View;
import android.widget.EditText;
import android.widget.ToggleButton;

public class MainActivity extends Activity {

    private ToggleButton mMeasureBtn;
    private EditText mGravityText;
    private SensorManager mSensorManager;
    private Sensor mGravity;
    private Sensor mMagnetic;

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);

        mMeasureBtn = (ToggleButton)findViewById(R.id.toggleButton);
        mMeasureBtn.setOnClickListener(mOnToggleMeasure);
        mGravityText = (EditText)findViewById(R.id.gravityText);
        mSensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);
        mGravity = mSensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
        mMagnetic = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
    }

    private View.OnClickListener mOnToggleMeasure = new View.OnClickListener() {
        @Override
        public void onClick(View v) {
            if (mMeasureBtn.isChecked()) {
                mSensorManager.registerListener(mOnMeasureGravity, mGravity, SensorManager.SENSOR_DELAY_NORMAL);
                mSensorManager.registerListener(mOnMeasureMagnetic, mMagnetic, SensorManager.SENSOR_DELAY_NORMAL);
            } else {
                mSensorManager.unregisterListener(mOnMeasureGravity);
                mInitialGravityDirection = null;

                mSensorManager.unregisterListener(mOnMeasureMagnetic);
                mInitialMagneticDirection = null;
            }
        }
    };

    private Vector3 mInitialGravityDirection;
    private Vector3 mCurGravityDirection;
    private SensorEventListener mOnMeasureGravity = new SensorEventListener() {
        @Override
        public void onSensorChanged(SensorEvent event) {
            if (mInitialGravityDirection == null) {
                mInitialGravityDirection = new Vector3(event.values);
                mGravityText.setText("0°");
            } else {
                mCurGravityDirection = new Vector3(event.values);
                mGravityText.setText(String.format("%f°", calculateAngle()));;
            }
        }

        @Override
        public void onAccuracyChanged(Sensor sensor, int accuracy) {

        }
    };

    private Vector3 mInitialMagneticDirection;
    private Vector3 mCurMagneticDirection;
    private SensorEventListener mOnMeasureMagnetic = new SensorEventListener() {
        @Override
        public void onSensorChanged(SensorEvent event) {
            if (mInitialMagneticDirection == null) {
                mInitialMagneticDirection = new Vector3(event.values);
            } else {
                mCurMagneticDirection = new Vector3(event.values);
            }
        }

        @Override
        public void onAccuracyChanged(Sensor sensor, int accuracy) {

        }
    };

    private double calculateAngle() {
        if (mCurGravityDirection == null || mCurMagneticDirection == null)
            return 0;

        Vector3 y0 = mInitialMagneticDirection.cpy().nor();
        Vector3 z0 = mInitialGravityDirection.cpy().scl(-1).nor();
        Vector3 x0 = y0.cpy().crs(z0);
        y0 = z0.cpy().crs(x0);

        Vector3 y1 = mCurMagneticDirection.cpy().nor();
        Vector3 z1 = mCurGravityDirection.cpy().scl(-1).nor();
        Vector3 x1 = y1.cpy().crs(z1);
        y1 = z1.cpy().crs(x1);

        Vector3 x10 = new Vector3(x1.dot(x0), x1.dot(y0), x1.dot(z0));
        Vector3 y10 = new Vector3(y1.dot(x0), y1.dot(y0), y1.dot(z0));
        Vector3 z10 = new Vector3(z1.dot(x0), z1.dot(y0), z1.dot(z0));
        Matrix4 lookat = new Matrix4().setToLookAt(z10.cpy().scl(-1), y10);
        Quaternion q = new Quaternion();
        lookat.getRotation(q, true);
        float angle = q.getAngle();
        if (angle > 180)
            angle = 360 - angle;

        return angle;
     }
}
